#ifndef CAMERABASEA_HPP
#define CAMERABASEA_HPP

#include <opencv2/opencv.hpp>

#include "camerabase.hpp"

#include "fiocv.hpp"

#ifdef TESTMEASURE
#include "findmarker.hpp"
#endif

//#define QTDISPLAY

#ifdef QTDISPLAY
#include <QLabel>
#endif

class CameraBaseA : public CameraBase
{
public:


    static bool FindCorners(std::vector<cv::Point2f> &corners, const cv::Mat &frame, cv::Size patternsize = cv::Size(6, 9), std::string winname="FF", int waitms=100)
    {
        cv::Mat gray;
        cv::cvtColor(frame, gray, CV_BGR2GRAY);

        //this will be filled by the detected corners
        //CALIB_CB_FAST_CHECK saves a lot of time on images
        //that do not contain any chessboard corners
        bool patternfound = cv::findChessboardCorners(gray, patternsize, corners,
                                                      cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE
                                                      //    + cv::CALIB_CB_FAST_CHECK
                                                      );

        if (!patternfound)
        {
            corners.clear();
            return false;
        }

        cv::cornerSubPix(gray, corners, cv::Size(5, 5), cv::Size(-1, -1),
                         cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

if(!winname.empty())
{
        cv::Mat canvas;
        frame.copyTo(canvas);
        cv::drawChessboardCorners(canvas, patternsize, cv::Mat(corners), patternfound);
         cv::imshow(winname, canvas);
         cv::waitKey(waitms);
}

        return true;
    }

    static void ChessBoardPos(std::vector<cv::Point3f> &posCB, int nx = 6, int ny = 9, float l = 0.04115)
    {
        posCB.assign(nx*ny, cv::Point3f(0,0,0));
        for (int i = 0;i < ny;i++)
        {
            for (int j = 0;j < nx;j++)
            {
                int idx = j + i*nx;

                posCB[idx].x = j*l;
                posCB[idx].y = i*l;
            }
        }
    }

    static void LoadImages(std::vector<cv::Mat> &ml, std::string folder="/home/odroid/agv/20170228_110158/")
    {
        std::vector<std::string> filename;
        ListFileName(folder,filename);
        ml.assign(filename.size(),cv::Mat());
        for(size_t i=0;i<ml.size();)
        {
            ml[i]=cv::imread(folder+filename[i]);
            if(ml[i].empty())
            {
                ml.erase(ml.begin()+i);
                filename.erase(filename.begin()+i);
            }
            else
            {
                i++;
            }
        }
    }


     static void LoadImagesExtractCorners(std::vector< std::vector<cv::Point2f> > &corners, std::vector< std::vector<cv::Point3f> > &posCB, cv::Size &imageSize,
                                          std::string folder="/home/odroid/agv/20170228_110158/", cv::Size patternsize = cv::Size(6, 9), float scale=1, float l=15, std::string winname="FF", int waitms=100)
     {
         std::vector<std::string> filename;
         ListFileName(folder,filename);


         if(!winname.empty())cv::namedWindow(winname,cv::WINDOW_NORMAL);
         corners.assign(filename.size(),std::vector<cv::Point2f>());

         for(size_t i=0;i<corners.size();)
         {
             cv::Mat m=cv::imread(folder+filename[i]);
             if(!m.empty())
             {
                 if(FindCorners(corners[i],m,patternsize, winname, waitms))
                 {
                     imageSize=m.size();
                     imageSize.height*=scale;
                     imageSize.width*=scale;
                     for(size_t j=0;j<corners[i].size();j++)
                     {
                         corners[i][j]=corners[i][j]*scale;
                     }
                     i++;
                     continue;
                 }
             }
                 corners.erase(corners.begin()+i);
                 filename.erase(filename.begin()+i);
         }

         if(!winname.empty())cv::destroyWindow(winname);

         posCB.assign(corners.size(), std::vector<cv::Point3f>());
         for(size_t i=0;i<posCB.size();i++)
         {
             ChessBoardPos(posCB[i], patternsize.width, patternsize.height, l);
         }

     }


     static double computeReprojectionErrors(
             const std::vector<std::vector<cv::Point3f> >& objectPoints,
             const std::vector<std::vector<cv::Point2f> >& imagePoints,
             const std::vector<cv::Mat>& rvecs, const std::vector<cv::Mat>& tvecs,
             const cv::Mat& cameraMatrix, const cv::Mat& distCoeffs,
             std::vector<float>& perViewErrors )
     {
         std::vector<cv::Point2f> imagePoints2;
         int i, totalPoints = 0;
         double totalErr = 0, err;
         perViewErrors.resize(objectPoints.size());

         for( i = 0; i < (int)objectPoints.size(); i++ )
         {
             cv::projectPoints(cv::Mat(objectPoints[i]), rvecs[i], tvecs[i],
                               cameraMatrix, distCoeffs, imagePoints2);
             std::cout<<"error["<<i<<"]=\n"<<cv::Mat(imagePoints[i])-cv::Mat(imagePoints2)<<"\n";
             err = norm(cv::Mat(imagePoints[i]), cv::Mat(imagePoints2), cv::NORM_L2);
             int n = (int)objectPoints[i].size();
             perViewErrors[i] = (float)std::sqrt(err*err/n);
             totalErr += err*err;
             totalPoints += n;
         }

         return std::sqrt(totalErr/totalPoints);
     }


    static double computeReprojectionErrorsF(
            const std::vector<std::vector<cv::Point3f> >& objectPoints,
            const std::vector<std::vector<cv::Point2f> >& imagePoints,
            const std::vector<cv::Mat>& rvecs, const std::vector<cv::Mat>& tvecs,
            const cv::Mat& cameraMatrix, const cv::Mat& distCoeffs,
            std::vector<float>& perViewErrors )
    {
        std::vector<cv::Point2f> imagePoints2;
        int i, totalPoints = 0;
        double totalErr = 0, err;
        perViewErrors.resize(objectPoints.size());

        for( i = 0; i < (int)objectPoints.size(); i++ )
        {
            cv::fisheye::projectPoints(cv::Mat(objectPoints[i]), imagePoints2, rvecs[i], tvecs[i],
                                       cameraMatrix, distCoeffs);
//            std::cout<<"error["<<i<<"]=\n"<<cv::Mat(imagePoints[i])-cv::Mat(imagePoints2)<<"\n";
            err = norm(cv::Mat(imagePoints[i]), cv::Mat(imagePoints2), cv::NORM_L2);
            int n = (int)objectPoints[i].size();
            perViewErrors[i] = (float)std::sqrt(err*err/n);
            totalErr += err*err;
            totalPoints += n;
        }

        return std::sqrt(totalErr/totalPoints);
    }

    static void Calib(cv::Mat &K, cv::Mat &distortion, const std::vector< std::vector<cv::Point2f> > &corners, const std::vector< std::vector<cv::Point3f> > &posCB, cv::Size imageSize)
    {

        //    K = cv::Mat::eye(3, 3, CV_64FC1);
        //        K=(cv::Mat_<double>(3,3)<<725,0,320,0,725,240,0,0,1);
        std::vector<cv::Mat> Rv, Tv;

        distortion.release();

        distortion.create(8, 1, CV_64FC1);
        distortion.setTo(0);

        double rms = cv::calibrateCamera( posCB, corners, imageSize, K,
                                          distortion, Rv, Tv,
                                          cv::CALIB_FIX_ASPECT_RATIO
                                          |cv::CALIB_FIX_K3
                                          //                                      |cv::CALIB_USE_INTRINSIC_GUESS
                                          //                                      |cv::CALIB_FIX_PRINCIPAL_POINT
                                          //                                      |cv::CALIB_RATIONAL_MODEL
                                          );


        printf("RMS error reported by calibrateCamera: %g\n", rms);

        std::cout << "K=" << K << '\n';
        std::cout << "distortion=" << distortion << '\n';

//        for(size_t i=0;i<Rv.size();i++) std::cout << "RT["<<i<<"]=" << Rv[i] << '\n'<< Tv[i] << '\n'<<std::flush;

        std::vector<float> pve;
        double rpe=computeReprojectionErrors(posCB,corners,Rv,Tv,K,distortion,pve);

        std::cout<<"rpe="<<rpe<<'\n'<<std::flush;

    }


    static void CalibF(cv::Mat &K, cv::Mat &distortion, const std::vector< std::vector<cv::Point2f> > &corners, const std::vector< std::vector<cv::Point3f> > &posCB, cv::Size imageSize)
    {

        //    K = cv::Mat::eye(3, 3, CV_64FC1);
        //        K=(cv::Mat_<double>(3,3)<<725,0,320,0,725,240,0,0,1);
        std::vector<cv::Mat> Rv, Tv;

        distortion.release();

        distortion.create(4, 1, CV_64FC1);
        distortion.setTo(0);

        double rms = cv::fisheye::calibrate( posCB, corners, imageSize, K,
                                             distortion, Rv, Tv,
                                             cv::fisheye::CALIB_FIX_SKEW
                                             |cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC
                                                                                   |cv::fisheye::CALIB_CHECK_COND
                                             //                                      |cv::fisheye::CALIB_USE_INTRINSIC_GUESS
                                             //                                      |cv::fisheye::CALIB_FIX_PRINCIPAL_POINT
                                             //                                             |cv::fisheye::CALIB_FIX_K1
//                                                                                          |cv::fisheye::CALIB_FIX_K2
                                                                                          |cv::fisheye::CALIB_FIX_K3
                                                                                          |cv::fisheye::CALIB_FIX_K4
                                             );

        printf("RMS error reported by calibrateCamera: %g\n", rms);

        std::cout << "K=" << K << '\n';
        std::cout << "distortion=" << distortion << '\n';

//        for(size_t i=0;i<Rv.size();i++) std::cout << "RT["<<i<<"]=" << Rv[i] << '\n'<< Tv[i] << '\n'<<std::flush;

        std::vector<float> pve;
        double rpe=computeReprojectionErrorsF(posCB,corners,Rv,Tv,K,distortion,pve);

        std::cout<<"rpe="<<rpe<<'\n'<<std::flush;

    }


    static void ScaleK(cv::Mat &K, double scalex, double scaley)
    {
        K.row(0)=K.row(0)*scalex;
        K.row(1)=K.row(1)*scaley;
    }

    static void OffsetK(cv::Mat &K, double offsetx, double offsety)
    {
        K.at<double>(0,2)+=offsetx;
        K.at<double>(1,2)+=offsety;
    }

    static void Calib(std::string folder="/home/odroid/agv/20170228_110158/")
    {

       std::vector< std::vector<cv::Point2f> > corners;
        std::vector< std::vector<cv::Point3f> > posCB;
        cv::Size imageSize;

        LoadImagesExtractCorners(corners, posCB, imageSize, folder);

        cv::Mat K,D;
        Calib(K, D, corners, posCB, imageSize);
        Save(K,folder+"Kl.yml");
        Save(D,folder+"Dl.yml");


         CalibF(K, D, corners, posCB, imageSize);

         Save(K,folder+"Kf.yml");
         Save(D,folder+"Df.yml");
    }


#if 0
    static void StereoCalib(cv::Vec6f &pos1to2,
                            const cv::Mat &K1, const cv::Mat &D1, const std::vector<cv::Mat> &ml1,
                            const cv::Mat &K2, const cv::Mat &D2, const std::vector<cv::Mat> &ml2,
                            cv::Size patternsize = cv::Size(6, 9), float l=15)
    {
        std::vector< std::vector<cv::Point2f> > corners1(ml1.size(),std::vector<cv::Point2f>());
        std::vector< std::vector<cv::Point2f> > corners2(ml2.size(),std::vector<cv::Point2f>());
        std::vector< std::vector<cv::Point3f> > posCB(ml1.size(), std::vector<cv::Point3f>());
        for(size_t i=0;i<posCB.size();i++)
        {
            FindCorners(corners1[i],ml1[i],patternsize);
            //                        cv::waitKey();
            FindCorners(corners2[i],ml2[i],patternsize);
            //                cv::waitKey();
            ChessBoardPos(posCB[i], patternsize.width, patternsize.height, l);

        }

        cv::Size imageSize=ml1[0].size();





        cv::Mat R,T,E,F;
        double rms=cv::stereoCalibrate(posCB,corners1,corners2,K1,D1,K2,D2,imageSize,R,T,E,F,
                                       cv::CALIB_FIX_INTRINSIC
                                       //                                   cv::CALIB_FIX_ASPECT_RATIO
                                       );

        std::cout<<"K1="<<K1<<'\n'<<std::flush;
        std::cout<<"K2="<<K2<<'\n'<<std::flush;

        std::cout<<"D1="<<D1<<'\n'<<std::flush;
        std::cout<<"D2="<<D2<<'\n'<<std::flush;
        std::cout<<"rms="<<rms<<'\n'<<std::flush;

        std::cout<<"R="<<R<<'\n'<<std::flush;
        std::cout<<"T="<<T<<'\n'<<std::flush;

        cv::Mat Rv;
        cv::Rodrigues(R,Rv);

        pos1to2[0]=Rv.at<double>(0);
        pos1to2[1]=Rv.at<double>(1);
        pos1to2[2]=Rv.at<double>(2);

        pos1to2[3]=T.at<double>(0);
        pos1to2[4]=T.at<double>(1);
        pos1to2[5]=T.at<double>(2);


        std::cout<<"pos1to2="<<pos1to2<<'\n'<<std::flush;

    }
#endif

#ifdef QTDISPLAY
    static void PostMat(const cv::Mat &img, QLabel *label)
    {

        //        QImage imdisplay((uchar*)img.data, img.cols, img.rows, img.step, QImage::Format_RGB888); //Converts the CV image into Qt standard format
        //        label->setPixmap(QPixmap::fromImage(imdisplay.rgbSwapped().copy()));//display the image in label that is created earlier

        if(img.channels()==3)
        {
            QImage imdisplay((uchar*)img.data, img.cols, img.rows, img.step, QImage::Format_RGB888); //Converts the CV image into Qt standard format
            label->setPixmap(QPixmap::fromImage(imdisplay.rgbSwapped().copy()));//display the image in label that is created earlier
        }
        else
        {
            QImage imdisplay((uchar*)img.data, img.cols, img.rows, img.step, QImage::Format_Mono); //Converts the CV image into Qt standard format
            label->setPixmap(QPixmap::fromImage(imdisplay).copy());//display the image in label that is created earlier
        }

        //        label->update();
        //        label->repaint();
    }
#endif

public:
    cv::Mat K;
    cv::Mat D;

    CameraBaseA()
        : CameraBase()
    {

    }

    void CenterCross(cv::Mat &canvas, cv::Scalar color=cv::Scalar(0,0,255))
    {
        cv::line(canvas,cv::Point(K.at<double>(2),0),cv::Point(K.at<double>(2),canvas.rows),color);
        cv::line(canvas,cv::Point(0,K.at<double>(5)),cv::Point(canvas.cols,K.at<double>(5)),color);
    }

    virtual void Output(double angle, double centerX, double centerY, int markerId)
    {

    }

#ifdef TESTMEASURE
    void CodeMeasure(int *keyPtr, std::string winname,
                     int dictionaryId=11, float markerLengthmm=30
        #ifdef QTDISPLAY
            , QLabel *label=NULL
        #endif
            )
    {
        FindMarker fm(dictionaryId);

        keyPtr[0]=0;
#ifdef DPIMG
#ifdef QTDISPLAY
#else
        cv::namedWindow(winname);
        cv::moveWindow(winname,100,100);
#endif
#endif
        while(keyPtr[0]!=27)
        {
            cv::Mat image;
            if(read(image))
            {
                double angle;
                double centerX;
                double centerY;
                int markerId;

                //                std::cout<<winname<<" load new\n"<<std::flush;

                if(fm.Detect(angle,centerX,centerY,markerId,image,K,D,markerLengthmm))
                {
#ifdef DPIMG
                    fm.DrawResult(image,K,D,markerLengthmm);
                    CenterCross(image,cv::Scalar(255,0,0));
#endif
                    Output(angle, centerX, centerY, markerId);
                }
                else
                {
#ifdef DPIMG
                    CenterCross(image,cv::Scalar(255,0,0));
#endif
                }
#ifdef DPIMG
#ifdef QTDISPLAY
                if(label!=NULL)
                    PostMat(image, label);
                //                cv::imwrite(winname+".png",image);

#else
                cv::imshow(winname,image);
                keyPtr[0]=cv::waitKey(1);
#endif
#endif

            }


        }
        release();
        std::cout<<winname<<" normal end\n"<<std::flush;
    }
#endif


};

#endif // CAMERABASEA_HPP
